#!/usr/bin/env python3
# -*- coding: utf-8 -*-
from __future__ import absolute_import
from __future__ import print_function
import rospy
# import csv
import math
from nav_msgs.msg import Odometry
from std_msgs.msg import Float32MultiArray, Int32
from geometry_msgs.msg import Twist

def leaderOdom_CB(data):
    allPosition[0] = float(data.pose.pose.position.x)
    allPosition[1] = float(data.pose.pose.position.y)
    allPosition[2] = float(data.twist.twist.linear.x)
    allPosition[3] = float(data.twist.twist.angular.z)

def follower1Odom_CB(data):
    allPosition[4] = float(data.pose.pose.position.x)
    allPosition[5] = float(data.pose.pose.position.y)
    allPosition[6] = float(data.twist.twist.linear.x)
    allPosition[7] = float(data.twist.twist.angular.z)

def follower2Odom_CB(data):
    allPosition[8] = float(data.pose.pose.position.x)
    allPosition[9] = float(data.pose.pose.position.y)
    allPosition[10] = float(data.twist.twist.linear.x)
    allPosition[11] = float(data.twist.twist.angular.z)
def flag_CB(data):
    allPosition[12] = float(data.data)
    # print(state)
def mpc_dif_CB(data):
    allPosition[13] = float(data.linear.z)
    allPosition[14] = float(data.linear.x)
    allPosition[15] = float(data.linear.y)
    allPosition[16] = float(data.angular.x)
    allPosition[17] = float(data.angular.y)

def publishAllLocations():
    # 初始化节点
    rospy.init_node('publish_states')
    # 创建发布器
    pub = rospy.Publisher('publishAllLocations', Float32MultiArray, queue_size=1)
    # 创建接收器

    simulation = "yes"
    print(("simulation: %s" %simulation))
    simulation = rospy.get_param('~simulation')
    print(("simulation: %s" %simulation))
    leader_odom_topic = '/tb3_0/odom'
    follower1_odom_topic = '/tb3_1/odom'
    follower2_odom_topic = '/tb3_2/odom'
    if (simulation == "no"):
        follower1_odom_topic = '/tb3_1/fake_odom'
        follower2_odom_topic = '/tb3_2/fake_odom'

    rospy.Subscriber(leader_odom_topic, Odometry, leaderOdom_CB,queue_size=1,buff_size=52428800)    # 接收领航者位置速度信息
    rospy.Subscriber(follower1_odom_topic, Odometry, follower1Odom_CB,queue_size=1,buff_size=52428800)    # 接收follower1位置速度信息
    rospy.Subscriber(follower2_odom_topic, Odometry, follower2Odom_CB,queue_size=1,buff_size=52428800)    # 接收follower2位置速度信息
    rospy.Subscriber('/cwm_flag', Int32, flag_CB, queue_size=1, buff_size=52428800) # 终点标志位
    rospy.Subscriber('/mpc_dif', Twist, mpc_dif_CB, queue_size=1, buff_size=52428800) # mpc差异证明

    # spin() simply keeps python from exiting until this node is stopped
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        pos = Float32MultiArray(data=allPosition) 
        pub.publish(pos)
        print("Publishing all locations")
        rate.sleep()

if __name__ == '__main__':
    # allPosition[0 ~ 3] "Leader X","Leader Y","Leader V_x","Leader W"
    # allPosition[4 ~ 7] "follower1 X","follower1 Y","follower1 V_x","follower1 W"
    # allPosition[8 ~ 11] "follower2 X","follower2 Y","follower2 V_x","follower2 W"
    # allPosition[12] "state" 是否到达终点
    # allPosition[13] "Controller mode" 控制器模式 0.0 为BCI控制  1.0 自主导航模块
    # allPosition[14~17] "mpc_v_before", "mpc_w_before", "mpc_v_after", "mpc_v_after"
    allPosition = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    try:
        publishAllLocations()
    except rospy.ROSInterruptException:
        pass
